﻿using System.Collections.Generic;
using UnityEngine;

//for more on A* visit
//https://en.wikipedia.org/wiki/A*_search_algorithm

namespace Core
{
    public class AStarPathfinder2
    {
        public IAStarGrid GridBase;
        public Node StartPosition;
        public Node EndPosition;

        public volatile bool JobDone;

        public void FindPath(ref List<Node> foundPath, Node start, Node target)
        {
            foundPath.Clear();

            StartPosition = start;
            EndPosition = target;

            FindPathActual(ref foundPath, StartPosition, EndPosition);

            JobDone = true;
        }

        private readonly List<Node> openList = new();
        private readonly HashSet<Node> closedSet = new();

        private void FindPathActual(ref List<Node> foundPath, Node start, Node target)
        {
            openList.Clear();
            closedSet.Clear();

            //We start adding to the open set
            openList.Add(start);

            while (openList.Count > 0)
            {
                Node currentNode = openList[0];

                for (int i = 0; i < openList.Count; i++)
                {
                    var n = openList[i];
                    //We check the costs for the current node
                    //You can have more opt. here but that's not important now
                    if (n.FCost < currentNode.FCost ||
                        (Mathf.Approximately(n.FCost, currentNode.FCost) &&
                        n.HCost < currentNode.HCost))
                    {
                        //and then we assign a new current node
                        if (!currentNode.Equals(n))
                        {
                            currentNode = n;
                        }
                    }
                }

                //we remove the current node from the open set and add to the closed set
                openList.Remove(currentNode);
                closedSet.Add(currentNode);

                //if the current node is the target node
                if (currentNode.Equals(target))
                {
                    //that means we reached our destination, so we are ready to retrace our path
                    RetracePath(ref foundPath, start, currentNode);
                    break;
                }

                //if we haven't reached our target, then we need to start looking the neighbours
                foreach (Node neighbour in GetNeighbours(currentNode))
                {
                    if (!closedSet.Contains(neighbour))
                    {
                        //we create a new movement cost for our neighbours
                        float newMovementCostToNeighbour = currentNode.GCost + GetDistance(currentNode, neighbour) + neighbour.Weight;

                        //and if it's lower than the neighbour's cost
                        if (newMovementCostToNeighbour < neighbour.GCost || !openList.Contains(neighbour))
                        {

                            if (!openList.Contains(neighbour))
                            {
                                //we calculate the new costs
                                neighbour.GCost = newMovementCostToNeighbour;
                                neighbour.HCost = GetDistance(neighbour, target);
                                //Assign the parent node
                                neighbour.ParentNode = currentNode;
                                //And add the neighbour node to the open set
                                if (!openList.Contains(neighbour))
                                {
                                    openList.Add(neighbour);
                                }
                            }
                        }
                    }
                }
            }
        }

        private void RetracePath(ref List<Node> foundPath, Node startNode, Node endNode)
        {
            //Retrace the path, is basically going from the endNode to the startNode
            Node currentNode = endNode;

            while (currentNode != startNode)
            {
                foundPath.Add(currentNode);
                //by taking the parentNodes we assigned
                currentNode = currentNode.ParentNode;
            }

            //then we simply reverse the list
            foundPath.Reverse();

        }

        private IEnumerable<Node> GetNeighbours(Node node)
        {
            for (int x = -1; x <= 1; x++)
            {
                for (int y = -1; y <= 1; y++)
                {
                    if (x == 0 && y == 0)
                    {
                        //00 is the current node
                        continue;
                    }

                    Node newNode = GetNeighbourNode(node.X + x, node.Y + y, node);

                    if (newNode == null)
                    {
                        continue;
                    }

                    yield return newNode;
                }
            }
        }

        private Node GetNeighbourNode(int adjX, int adjZ, Node currentNodePos)
        {
            //this is where the meat of it is
            //We can add all the checks we need here to tweak the algorythm to our heart's content
            //but first let's start from the usual stuff you'll see in A*

            Node retVal = null;

            //let's take the node from the adjacent positions we passed
            var node = GetNode(adjX, adjZ);

            //if it's not null and we can walk on it
            if (node is { IsWalkable: true })
            {
                //we can use that node
                retVal = node;
            }

            ////if the node is diagonal to the current node then check the neighbouring nodes
            ////so to move diagonally, we need to have 4 nodes walkable
            //int originalX = adj_x - currentNodePos.x;
            //int originalZ = adj_z - currentNodePos.y;

            //if (Mathf.Abs(originalX) == 1 && Mathf.Abs(originalZ) == 1)
            //{
            //    // the first block is originalX, 0 and the second to check is 0, originalZ
            //    //They need to be pathfinding walkable
            //    Node neighbour1 = GetNode(currentNodePos.x + originalX, currentNodePos.y);
            //    if (neighbour1 == null || !neighbour1.isWalkable)
            //    {
            //        retVal = null;
            //    }

            //    Node neighbour2 = GetNode(currentNodePos.x, currentNodePos.y + originalZ);
            //    if (neighbour2 == null || !neighbour2.isWalkable)
            //    {
            //        retVal = null;
            //    }
            //}

            return retVal;
        }

        private Node GetNode(int x, int z)
        {
            Node n;

            lock (GridBase)
            {
                n = GridBase.GetNode(x, z);
            }
            return n;
        }

        private float GetDistance(Node posA, Node posB)
        {
            //We find the distance between each node
            //not much to explain here

            int distX = Mathf.Abs(posA.X - posB.X);
            int distZ = Mathf.Abs(posA.Y - posB.Y);

            if (distX > distZ)
            {
                return 1.414f * distZ + (distX - distZ);
            }

            return 1.414f * distX + (distZ - distX);
            //return Mathf.Sqrt(distX * distX + distZ * distZ);
        }
    }
}
